# main - By: LLZ - 周四 7月 28 2022
# 1) OpenMV Cam 与 Arduino Uno 按如下连线:
#
# OpenMV Cam Ground Pin   ----> MSP430 Ground
# OpenMV Cam UART3_TX(P4) ----> MSP430 UART_RX
# OpenMV Cam UART3_RX(P5) ----> MSP430 UART_TX
# mode 0 sigle line
# mode 1 cut in
# mode 2 cut out
# stop_flag (0, no; 1, yes)
# Edge detection with Canny:
import sensor, image, time, math, pyb
from pyb import UART

def a_scale(a):
    return round((a + 1) * 50)

def b_scale(b):
    factor = img.width() / 100
    return round(b / factor)

def line_to_theta_and_rho(line):
    if line.rho() < 0: # quadrant 3/4
        if line.theta() < 90: # quadrant 3 (unused)
            return (math.sin(math.radians(line.theta())),
                math.cos(math.radians(line.theta() + 180)) * -line.rho())
        else: # quadrant 4
            return (math.sin(math.radians(line.theta() - 180)),
                math.cos(math.radians(line.theta() + 180)) * -line.rho())
    else: # quadrant 1/2
        if line.theta() < 90: # quadrant 1
            return (math.sin(math.radians(180 - line.theta())),
                math.cos(math.radians(line.theta())) * line.rho())
        else: # quadrant 2
            return (math.sin(math.radians(line.theta() - 180)),
                math.cos(math.radians(line.theta()) + 180) * line.rho())

def line_to_theta_and_rho_error(line, img):
    t, r = line_to_theta_and_rho(line)
    return (t, r - (img.width() // 2))

sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.GRAYSCALE) # or sensor.RGB565
sensor.set_framesize(sensor.QQVGA) # or sensor.QVGA (or others)
sensor.skip_frames(time = 2000) # Let new settings take affect.
sensor.set_gainceiling(8)
kernel_size = 1 # 3x3==1, 5x5==2, 7x7==3, etc.
kernel = [-1,  0,  1, \
          -2,  0,  2, \
          -1,  0,  1]
kernel_mul = 1;
# UART 3, and baudrate.
uart = UART(3, 9600)
clock = time.clock() # Tracks FPS.
led_g = pyb.LED(2) # Red LED = 1, Green LED = 2, Blue LED = 3, IR LEDs = 4.
led_r = pyb.LED(1)
centel_final = 0
while(True):
    clock.tick() # Track elapsed milliseconds between snapshots().
    img = sensor.snapshot() # Take a picture and return the image.
    #img2 = img.crop(copy=True)
    # Use Canny edge detector
    img.morph(kernel_size, kernel, kernel_mul)
    img.binary([(200, 255)])
    #img.erode(2)
    img.find_edges(image.EDGE_CANNY, threshold=(200, 255))
    #img.dilate(2)
    #img.find_edges(image.EDGE_CANNY, threshold=(255, 255))

    line = img.get_regression([(255, 255)], robust=True)
    if line == None: #no line is detected
        continue
    x_avr = ((line.x1() + line.x2()) / 2)
    centel_final = round(x_avr * 0.3 + centel_final * 0.7);
    ROI_line_l = [0, 0, centel_final, img.height()]
    ROI_line_r = [centel_final, 0, img.width() - centel_final, img.height()]
    line_l = img.get_regression([(255, 255)], robust=True, roi=ROI_line_l)
    line_r = img.get_regression([(255, 255)], robust=True, roi=ROI_line_r)
    mode = 0
    if line_l == None or line_r == None:
        mode = 0
    else:
        [a_l, b_l] = line_to_theta_and_rho(line_l)
        [a_r, b_r] = line_to_theta_and_rho(line_r)
        a_l = a_scale(a_l)
        b_l = b_scale(b_l)
        a_r = a_scale(a_r)
        b_r = b_scale(b_r)
        img.draw_line(line_l.line())
        img.draw_line(line_r.line())
        #img.draw_string(0, 20, str(a_l)+","+str(b_l)+","+str(line_l.magnitude()))
        #img.draw_string(0, 40, str(a_r)+","+str(b_r)+","+str(line_r.magnitude()))
        if line_l.magnitude() > 30 and line_r.magnitude() > 30 and a_r > a_l and (a_r - a_l > 12):
            led_g.on()
        else:
            led_g.off()
    [a,b] = line_to_theta_and_rho(line)
    ascale = a_scale(a)
    bscale = b_scale(x_avr)
    uart_send = str(ascale)+","+str(bscale)+"\n"
    img.draw_line(line.line())
    #img.draw_string(0, 20, str(line.theta())+","+str(line.rho()))
    img.draw_string(0, 0, str(ascale)+","+str(bscale)+","+str(line.magnitude()))

    #print(line.line())
    print(clock.fps()) # Note: Your OpenMV Cam runs about half as fast while
    uart.write(uart_send)



